#ifndef CLASS_TREEFORCEFINDER
#define CLASS_TREEFORCEFINDER

//#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <cstdlib> // std::system("...");
#include <sys/stat.h>
#include <sys/time.h>
#include <time.h>
#include <math.h>
#include <string.h>
#include <string>
#include <vector>

typedef unsigned int uint;
using namespace std;


class TreeForceFinder {
	public:
			
	double ErrTolTheta;
	double softening;
	int NumPart;
	
	double G;
	
	TreeForceFinder():
	ErrTolTheta(0.35),
	softening(0.1),
	NumPart(0),
	G(1)
	{
		//printf("TREE_FORCE_FINDER is initialized\n");
	}
	
	struct NODE {
		float len;			/* sidelength of treenode */
		float center[3];		/* geometrical center */
		union {
			int suns[8];		/* ancestor nodes */
			struct {
				float s[3];
				float mass;
				int sibling;
				int nextnode;
			} d;
		} u;
	} *Nodes, *Nodes_base;
	
	int MaxNodes;		/* maximum allowed number of internal nodes */
	int MaxPart;
	int *Nextnode;		/* gives next node in tree walk  (nodes array) */

	int last;		/* auxialiary variable used to set-up non-recursive walk */
	
	template <typename T>
	int force_treebuild(T *p) {
		
			printf("TREE_FORCE_FINDER::force_treebuild(T *p)...");
			
			int i, j, subnode = 0, parent = -1, numnodes;
			int nfree, th, nn;
			double lenhalf;
			struct NODE *nfreep;
			double len, xmin[3], xmax[3];


			Nodes = Nodes_base - MaxPart;

			/* select first node */
			nfree = MaxPart;		/* index */
			nfreep = &Nodes[nfree];

			/* create an empty  root node  */

			for(j = 0; j < 3; j++)	/* find enclosing rectangle */
				xmin[j] = xmax[j] = p[0].Pos[j];

			for(i = 1; i < NumPart; i++)
				for(j = 0; j < 3; j++) {
					if(p[i].Pos[j] > xmax[j]) xmax[j] = p[i].Pos[j];
					if(p[i].Pos[j] < xmin[j]) xmin[j] = p[i].Pos[j];
				}

			for(j = 1, len = xmax[0] - xmin[0]; j < 3; j++)	/* determine maxmimum extension */
				if((xmax[j] - xmin[j]) > len) len = xmax[j] - xmin[j];

			for(j = 0; j < 3; j++)
				nfreep->center[j] = (xmin[j] + xmax[j]) / 2;
		
			nfreep->len = len;

			for(i = 0; i < 8; i++)
				nfreep->u.suns[i] = -1;

			numnodes = 1;
			nfreep++;
			nfree++;

			nfreep = &Nodes[nfree];
	
		for(i = 0; i < NumPart; i++)	{ // insert all particles 
				
			th = MaxPart;
			while(1) {
				if(th >= MaxPart) {	/* we are dealing with an internal node */
								
						subnode = 0;
						if(p[i].Pos[0] > Nodes[th].center[0])	subnode += 1;
						if(p[i].Pos[1] > Nodes[th].center[1]) subnode += 2;
						if(p[i].Pos[2] > Nodes[th].center[2]) subnode += 4;

						nn = Nodes[th].u.suns[subnode];

						if (nn >= 0)	{// ok, something is in the daughter slot already, need to continue 
							parent = th;	/* note: subnode can still be used in the next step of the walk */
							th = nn;
						} else {
							/* here we have found an empty slot where we can 
							* attach the new particle as a leaf 
								*/
							Nodes[th].u.suns[subnode] = i;
							break;	/* done for this particle */
						}
				}
				else
				{
						/* we try to insert into a leaf with a single particle
						* need to generate a new internal node at this point 
						*/
					Nodes[parent].u.suns[subnode] = nfree;

					nfreep->len = 0.5 * Nodes[parent].len;
					lenhalf = 0.25 * Nodes[parent].len;

					if(subnode & 1) nfreep->center[0] = Nodes[parent].center[0] + lenhalf;
					else nfreep->center[0] = Nodes[parent].center[0] - lenhalf;

					if(subnode & 2) nfreep->center[1] = Nodes[parent].center[1] + lenhalf;
					else nfreep->center[1] = Nodes[parent].center[1] - lenhalf;

					if(subnode & 4) nfreep->center[2] = Nodes[parent].center[2] + lenhalf;
					else nfreep->center[2] = Nodes[parent].center[2] - lenhalf;

					nfreep->u.suns[0] = -1;
					nfreep->u.suns[1] = -1;
					nfreep->u.suns[2] = -1;
					nfreep->u.suns[3] = -1;
					nfreep->u.suns[4] = -1;
					nfreep->u.suns[5] = -1;
					nfreep->u.suns[6] = -1;
					nfreep->u.suns[7] = -1;

					subnode = 0;
					if(p[th].Pos[0] > nfreep->center[0]) subnode += 1;
					if(p[th].Pos[1] > nfreep->center[1]) subnode += 2;
					if(p[th].Pos[2] > nfreep->center[2]) subnode += 4;

					nfreep->u.suns[subnode] = th;

					th = nfree;	// resume trying to insert the new particle at the newly created internal node 

					numnodes++;
					nfree++;
					nfreep++;

					if((numnodes) >= MaxNodes)	{
						printf("maximum number %d of tree-nodes reached.\n", MaxNodes);
						printf("for particle %d\n", i);
						exit(1);
					}
				}
			}
		}

		// now compute the multipole moments recursively 
		last = -1;
		force_update_node_recursive(p, MaxPart, -1);

		if(last >= MaxPart)
			Nodes[last].u.d.nextnode = -1;
		else
			Nextnode[last] = -1;

		printf("done\n\n");
		
		return numnodes;
	}

	/*
	void force_treeevaluate() {
		
		for(uint i=0; i<NumPart; i++)
			force_treeevaluate(p[i].Pos, p[i].acc);
		
	}
	*/
	template <typename T>
	int force_treeevaluate(T *p, REAL *pos, REAL *acc, REAL &pot) {
	
		struct NODE *nop = 0;
		int no, ninteractions;
		double r2, dx, dy, dz, mass, r, fac, u, h, h_inv, h3_inv;
		double acc_x, acc_y, acc_z, pos_x, pos_y, pos_z;

		acc_x = 0;
		acc_y = 0;
		acc_z = 0;
		ninteractions = 0;

		pos_x = pos[0];
		pos_y = pos[1];
		pos_z = pos[2];
		
		double pot_(0);

		h = 2.8 * softening;
		h_inv = 1.0 / h;
		h3_inv = h_inv * h_inv * h_inv;

		no = MaxPart;			// first node 

		while(no >= 0) {
			if(no < MaxPart) {
				// the index of the node is the index of the particle 
				dx = p[no].Pos[0] - pos_x;
				dy = p[no].Pos[1] - pos_y;
				dz = p[no].Pos[2] - pos_z;

				r2 = dx * dx + dy * dy + dz * dz;

				mass = p[no].Mass;

				no = Nextnode[no];
				
			}  else	{ // we have an  internal node 
				
				nop = &Nodes[no];

				dx = nop->u.d.s[0] - pos_x;
				dy = nop->u.d.s[1] - pos_y;
				dz = nop->u.d.s[2] - pos_z;

				r2 = dx*dx + dy*dy + dz*dz;

				mass = nop->u.d.mass;

				//printf("nop->len=%g  r=%g  %g %g\n", nop->len, sqrt(r2), nop->len * nop->len , r2 * ErrTolTheta * ErrTolTheta);

				if(nop->len * nop->len > r2 * ErrTolTheta * ErrTolTheta)	{
					no = nop->u.d.nextnode; // open cell 
					continue;
				}

				// check in addition whether we lie inside the cell 

				if(fabs(nop->center[0] - pos_x) < 0.60 * nop->len)
					if(fabs(nop->center[1] - pos_y) < 0.60 * nop->len)
						if(fabs(nop->center[2] - pos_z) < 0.60 * nop->len)	{
							no = nop->u.d.nextnode;
							continue;
						}

				no = nop->u.d.sibling;	// ok, node can be used 
			}

			r = sqrt(r2);

			if(r >= h) fac = mass / (r2 * r);
			else {
				u = r * h_inv;
				if(u < 0.5)  fac = mass * h3_inv * (10.666666666667 + u * u * (32.0 * u - 38.4));
				else fac = mass * h3_inv * (21.333333333333 - 48.0 * u +
			       38.4 * u * u - 10.666666666667 * u * u * u - 0.066666666667 / (u * u * u));
			}

			acc_x += dx * fac;
			acc_y += dy * fac;
			acc_z += dz * fac;

			pot_+= -fac*r2;
			
			ninteractions++;
		}

		// store result at the proper place 
		
		acc[0] = G*acc_x;
		acc[1] = G*acc_y;
		acc[2] = G*acc_z;

		pot = G*pot_;
		
		return ninteractions;
	}

	/* this function allocates memory used for storage of the tree
	 * and auxiliary arrays for tree-walk and link-lists.
	*/
	void force_treeallocate(int maxnodes, int maxpart) {	// usually maxnodes=0.7*maxpart is sufficient 
	
		printf("TREE_FORCE_FINDER::force_treeallocate(..)...\n", maxnodes,maxpart);
		
		size_t bytes, allbytes = 0;

		MaxNodes = maxnodes;
		MaxPart = maxpart;

		if(!(Nodes_base = (NODE*)malloc(bytes = (MaxNodes + 1) * sizeof(struct NODE)))) {
			printf("failed to allocate memory for %d tree-nodes (%g Mbytes).\n", MaxNodes, bytes/1024.0/1024.0);
			exit(3);
		}
		allbytes += bytes;

		if(!(Nextnode = (int*)malloc(bytes = (maxpart) * sizeof(int)))) {
			printf("Failed to allocate %d spaces for 'Nextnode' array (%g Mbytes)\n", maxpart, bytes/1024.0/1024.0);
			exit(0);
		}
		
		allbytes += bytes;

		printf("\tUse %g MByte for BH-tree. Done\n\n", allbytes / (1024.0 * 1024.0));
	}


	/* free the allocated memory  */
	void force_treefree(void) {
		free(Nextnode);
		free(Nodes_base);
	}
	
private:

	template <typename T>
	void force_update_node_recursive(T *p, int no, int sib) {
  
		int j, jj, _p, pp = 0, nextsib, suns[8];
		double s[3], mass;

		if(no >= MaxPart) {		// internal node 
    
			for(j = 0; j < 8; j++)
					suns[j] = Nodes[no].u.suns[j]; // this "backup" is necessary because the nextnode entry will overwrite one element (union!)
		
		if(last >= 0) {
			if(last >= MaxPart)
				Nodes[last].u.d.nextnode = no;
			else
				Nextnode[last] = no;
		}

      last = no;

      mass = 0;
      s[0] = 0;
      s[1] = 0;
      s[2] = 0;

      for(j = 0; j < 8; j++) {
			if((_p = suns[j]) >= 0) {
				
				// check if we have a sibling on the same level
				for(jj = j + 1; jj < 8; jj++)
				if((pp = suns[jj]) >= 0) break;

	      if(jj < 8)	// yes, we do 
		nextsib = pp;
	      else
		nextsib = sib;

	      force_update_node_recursive(p, _p, nextsib);

	      if(_p >= MaxPart)	// an internal node or pseudo particle 
		{
		  mass += Nodes[_p].u.d.mass;	// we assume a fixed particle mass 
		  s[0] += Nodes[_p].u.d.mass * Nodes[_p].u.d.s[0];
		  s[1] += Nodes[_p].u.d.mass * Nodes[_p].u.d.s[1];
		  s[2] += Nodes[_p].u.d.mass * Nodes[_p].u.d.s[2];
		}
	      else		// a particle 
		{
		  mass += p[_p].Mass;
		  s[0] += p[_p].Mass * p[_p].Pos[0];
		  s[1] += p[_p].Mass * p[_p].Pos[1];
		  s[2] += p[_p].Mass * p[_p].Pos[2];
		}
	    }
	}

      if(mass)
	{
	  s[0] /= mass;
	  s[1] /= mass;
	  s[2] /= mass;
	}
      else
	{
	  s[0] = Nodes[no].center[0];
	  s[1] = Nodes[no].center[1];
	  s[2] = Nodes[no].center[2];
	}

      Nodes[no].u.d.s[0] = s[0];
      Nodes[no].u.d.s[1] = s[1];
      Nodes[no].u.d.s[2] = s[2];
      Nodes[no].u.d.mass = mass;

      Nodes[no].u.d.sibling = sib;
    }
  else				// single particle 
    {
      if(last >= 0)
	{
	  if(last >= MaxPart)
	    Nodes[last].u.d.nextnode = no;
	  else
	    Nextnode[last] = no;
	}

			last = no;
		}
	}
	
	
};	

#endif